{
 "metadata": {
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7-final"
  },
  "orig_nbformat": 2,
  "kernelspec": {
   "name": "python3",
   "display_name": "Python 3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2,
 "cells": [
  {
   "source": [
    "# 03 加入简单控制器\n",
    "\n",
    "在无驱动仿真流程的基础上，在框图中加入简单控制器模块。"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "source": [
    "## 重复加载机器人的过程"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
    "\n",
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator,\n",
    "    # add for ground model\n",
    "    RigidTransform, CoulombFriction, HalfSpace,\n",
    "    # add for single trunk model\n",
    "    #GeometryFrame, Box, Sphere, GeometryInstance, MakePhongIllustrationProperties\n",
    ")\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "source": [
    "导入控制器"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "sys.path.append(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/anymal_control\")\n",
    "\n",
    "from controllers import MPTCController, IDController, BasicController"
   ]
  },
  {
   "source": [
    "仿真过程的设置变量\n"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "use_lcm = False\n",
    "\n",
    "sim_time = 6.0\n",
    "dt = 5e-3\n",
    "target_realtime_rate = 1.0\n"
   ]
  },
  {
   "source": [
    "加载机器人和地面，同时加载简单控制器模块。\n",
    "\n",
    "加载的配置在调用builder.Build()之前完成。"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 63,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6002...\nYou can open the visualizer by visiting the following URL:\nhttp://127.0.0.1:7002/static/\nConnected to meshcat-server.\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "b692483e58214075bec5105bc4f8b302"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fl_to_thigh_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "98a11e13a8194afd92ad29e149f454c2"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fl_to_knee_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "9dca41a266284630ad8b534a1abc5c6a"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "d023d012630e4e66923a436253e31744"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fr_to_thigh_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "5d591909adf848298f31ca0e88d81b7b"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fr_to_knee_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "9786806a655a4cd38f5417d75b21cb55"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "bd04d1f6d55745de9ba344833d17e485"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hl_to_thigh_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "922712d4ff2546d78a96d33837f21cad"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hl_to_knee_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "08961b31c079424da178eb62db4d6b7f"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "55cda1a108234c959cf0571eee4ee38a"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hr_to_thigh_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "e02aeafb830c48169112bdeb9caf59ab"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hr_to_knee_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "1d1e5c4e1e234c4cb1e6e26737a9d8c7"
      }
     },
     "metadata": {}
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-3)\n",
    "\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"../models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "quad = Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "\n",
    "# Add a flat ground with friction\n",
    "X_BG = RigidTransform()\n",
    "surface_friction = CoulombFriction(\n",
    "        static_friction = 1.0,\n",
    "        dynamic_friction = 1.0)\n",
    "plant.RegisterCollisionGeometry(\n",
    "        plant.world_body(),      # the body for which this object is registered\n",
    "        X_BG,                    # The fixed pose of the geometry frame G in the body frame B\n",
    "        HalfSpace(),             # Defines the geometry of the object\n",
    "        \"ground_collision\",      # A name\n",
    "        surface_friction)        # Coulomb friction coefficients\n",
    "plant.RegisterVisualGeometry(\n",
    "        plant.world_body(),\n",
    "        X_BG,\n",
    "        HalfSpace(),\n",
    "        \"ground_visual\",\n",
    "        np.array([0.5,0.5,0.5,0.0]))    # Color set to be completely transparent\n",
    "\n",
    "# Turn off gravity\n",
    "#g = plant.mutable_gravity_field()\n",
    "#g.set_gravity_vector([0,0,0])\n",
    "\n",
    "plant.Finalize()\n",
    "assert plant.geometry_source_is_registered()\n",
    "\n",
    "\n",
    "controller = builder.AddSystem(BasicController(plant,dt,use_lcm=use_lcm))\n",
    "\n",
    "# Connect the controller to the simulated plant\n",
    "builder.Connect(controller.GetOutputPort(\"quad_torques\"),\n",
    "                plant.get_actuation_input_port(quad))\n",
    "builder.Connect(plant.get_state_output_port(),\n",
    "                controller.GetInputPort(\"quad_state\"))\n",
    "\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "diagram_context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, diagram_context);\n"
   ]
  },
  {
   "source": [
    "## 查看当前系统的结构图\n",
    "\n",
    "可以看到结构图中多了一个控制器模块。\n",
    "其输入为四足机器人的状态信息，输出为四足机器人的驱动力矩。"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ],
      "image/svg+xml": "<svg height=\"506pt\" viewBox=\"0.00 0.00 1404.00 506.45\" width=\"1404pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 502.45)\">\n<title>_94776164131008</title>\n<polygon fill=\"white\" points=\"-4,4 -4,-502.45 1400,-502.45 1400,4 -4,4\" stroke=\"transparent\"/>\n<g class=\"cluster\" id=\"clust1\">\n<title>cluster94776164131008diagram</title>\n<polygon fill=\"none\" points=\"8,-19.45 8,-490.45 1388,-490.45 1388,-19.45 8,-19.45\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"698\" y=\"-475.25\">94776164131008</text>\n</g>\n<g class=\"cluster\" id=\"clust4\">\n<title>cluster94776164131008subsystems</title>\n<polygon fill=\"none\" points=\"16,-27.45 16,-459.45 1380,-459.45 1380,-27.45 16,-27.45\" stroke=\"white\"/>\n</g>\n<!-- 94776162395584 -->\n<g class=\"node\" id=\"node1\">\n<title>94776162395584</title>\n<polygon fill=\"none\" points=\"24,-36.45 24,-450.45 655,-450.45 655,-36.45 24,-36.45\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-435.25\">plant</text>\n<polyline fill=\"none\" points=\"24,-427.45 655,-427.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-391.25\">geometry_query</text>\n<polyline fill=\"none\" points=\"24,-362.45 275,-362.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-326.25\">WorldModelInstance_actuation</text>\n<polyline fill=\"none\" points=\"24,-297.45 275,-297.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-261.25\">DefaultModelInstance_actuation</text>\n<polyline fill=\"none\" points=\"24,-232.45 275,-232.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-196.25\">mini_cheetah_actuation</text>\n<polyline fill=\"none\" points=\"24,-167.45 275,-167.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-131.25\">applied_generalized_force</text>\n<polyline fill=\"none\" points=\"24,-102.45 275,-102.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-65.75\">applied_spatial_force</text>\n<polyline fill=\"none\" points=\"275,-36.45 275,-427.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-412.25\">geometry_pose</text>\n<polyline fill=\"none\" points=\"275,-404.45 655,-404.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-389.25\">continuous_state</text>\n<polyline fill=\"none\" points=\"275,-381.45 655,-381.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-366.25\">body_poses</text>\n<polyline fill=\"none\" points=\"275,-358.45 655,-358.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-343.25\">spatial_velocities</text>\n<polyline fill=\"none\" points=\"275,-335.45 655,-335.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-320.25\">spatial_accelerations</text>\n<polyline fill=\"none\" points=\"275,-312.45 655,-312.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-297.25\">generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-289.45 655,-289.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-274.25\">WorldModelInstance_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-266.45 655,-266.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-251.25\">WorldModelInstance_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-243.45 655,-243.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-228.25\">DefaultModelInstance_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-220.45 655,-220.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-205.25\">DefaultModelInstance_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-197.45 655,-197.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-182.25\">mini_cheetah_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-174.45 655,-174.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-159.25\">mini_cheetah_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-151.45 655,-151.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-136.25\">WorldModelInstance_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-128.45 655,-128.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-113.25\">DefaultModelInstance_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-105.45 655,-105.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-90.25\">mini_cheetah_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-82.45 655,-82.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-67.25\">reaction_forces</text>\n<polyline fill=\"none\" points=\"275,-59.45 655,-59.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-44.25\">contact_results</text>\n</g>\n<!-- 94776161778000 -->\n<g class=\"node\" id=\"node2\">\n<title>94776161778000</title>\n<polygon fill=\"none\" points=\"816,-381.95 816,-450.95 1050,-450.95 1050,-381.95 816,-381.95\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"933\" y=\"-435.75\">scene_graph</text>\n<polyline fill=\"none\" points=\"816,-427.95 1050,-427.95 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"863\" y=\"-401.25\">plant_pose</text>\n<polyline fill=\"none\" points=\"910,-381.95 910,-427.95 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"980\" y=\"-412.75\">lcm_visualization</text>\n<polyline fill=\"none\" points=\"910,-404.95 1050,-404.95 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"980\" y=\"-389.75\">query</text>\n</g>\n<!-- 94776162395584&#45;&gt;94776161778000 -->\n<g class=\"edge\" id=\"edge1\">\n<title>94776162395584:y0-&gt;94776161778000:u0</title>\n<path d=\"M655,-416.45C722.83,-416.45 741.86,-405.6 804.93,-404.54\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"805.03,-408.04 815,-404.45 804.97,-401.04 805.03,-408.04\" stroke=\"black\"/>\n</g>\n<!-- 94776162535920 -->\n<g class=\"node\" id=\"node3\">\n<title>94776162535920</title>\n<polygon fill=\"none\" points=\"691,-229.95 691,-298.95 1175,-298.95 1175,-229.95 691,-229.95\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"933\" y=\"-283.75\">controllers.basic_controller.BasicController@00005632cb8049f0</text>\n<polyline fill=\"none\" points=\"691,-275.95 1175,-275.95 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"804\" y=\"-249.25\">quad_state</text>\n<polyline fill=\"none\" points=\"917,-229.95 917,-275.95 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1046\" y=\"-260.75\">quad_torques</text>\n<polyline fill=\"none\" points=\"917,-252.95 1175,-252.95 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1046\" y=\"-237.75\">output_metrics</text>\n</g>\n<!-- 94776162395584&#45;&gt;94776162535920 -->\n<g class=\"edge\" id=\"edge5\">\n<title>94776162395584:y1-&gt;94776162535920:u0</title>\n<path d=\"M655,-393.45C715.76,-393.45 637.05,-269.01 680.92,-253.94\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"681.62,-257.38 691,-252.45 680.6,-250.45 681.62,-257.38\" stroke=\"black\"/>\n</g>\n<!-- 94776161778000&#45;&gt;94776162395584 -->\n<g class=\"edge\" id=\"edge2\">\n<title>94776161778000:y1-&gt;94776162395584:u0</title>\n<path d=\"M980,-380.45C980,-232.65 799.75,-421.05 655,-450.95 586.28,-465.15 73.56,-500.12 23.5,-450.95 8.79,-436.5 0.73,-407.43 12.9,-398.28\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"13.89,-401.63 22.5,-395.45 11.92,-394.92 13.89,-401.63\" stroke=\"black\"/>\n</g>\n<!-- 94776162486896 -->\n<g class=\"node\" id=\"node4\">\n<title>94776162486896</title>\n<polygon fill=\"none\" points=\"1211,-405.45 1211,-451.45 1372,-451.45 1372,-405.45 1211,-405.45\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1291.5\" y=\"-436.25\">meshcat_visualizer</text>\n<polyline fill=\"none\" points=\"1211,-428.45 1372,-428.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1281\" y=\"-413.25\">lcm_visualization</text>\n<polyline fill=\"none\" points=\"1351,-405.45 1351,-428.45 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1361.5\" y=\"-413.25\"> </text>\n</g>\n<!-- 94776161778000&#45;&gt;94776162486896 -->\n<g class=\"edge\" id=\"edge4\">\n<title>94776161778000:y0-&gt;94776162486896:u0</title>\n<path d=\"M1051,-416.45C1118.64,-416.45 1138.03,-416.45 1200.95,-416.45\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"1201,-419.95 1211,-416.45 1201,-412.95 1201,-419.95\" stroke=\"black\"/>\n</g>\n<!-- 94776162535920&#45;&gt;94776162395584 -->\n<g class=\"edge\" id=\"edge3\">\n<title>94776162535920:y0-&gt;94776162395584:u3</title>\n<path d=\"M1176,-264.45C1191.56,-264.45 1186.16,-288.61 1175,-299.45 1097.87,-374.41 772.04,-370.17 691,-299.45 601.94,-221.74 744.06,-113.67 655,-35.95 602.13,10.18 73.27,13.51 23.5,-35.95 12.43,-46.96 6.23,-159.8 16.24,-191.32\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"13.63,-193.66 22.5,-199.45 19.17,-189.39 13.63,-193.66\" stroke=\"black\"/>\n</g>\n</g>\n</svg>"
     },
     "metadata": {},
     "execution_count": 18
    }
   ],
   "source": [
    "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
   ]
  },
  {
   "source": [
    "## 运行仿真\n"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 64,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f1c3b1eb770>"
      ]
     },
     "metadata": {},
     "execution_count": 64
    }
   ],
   "source": [
    "# Simulator setup\n",
    "simulator = Simulator(diagram, diagram_context)\n",
    "if use_lcm:\n",
    "    # If we're using LCM to send messages to another simulator or a real\n",
    "    # robot, we don't want Drake to slow things down, so we'll publish\n",
    "    # new messages as fast as possible\n",
    "    simulator.set_target_realtime_rate(0.0)\n",
    "else:\n",
    "    simulator.set_target_realtime_rate(target_realtime_rate)\n",
    "    \n",
    "# Set initial states\n",
    "plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)\n",
    "\n",
    "q0 = np.asarray([ 1.0, 0.0, 0.0, 0.0,     # base orientation\n",
    "                  0.0, 0.0, 0.35,          # base position\n",
    "                  0.0, 0.0, 0.0, 0.0,     # ad/ab\n",
    "                 -0.8,-0.8,-0.8,-0.8,     # hip\n",
    "                  1.6, 1.6, 1.6, 1.6])    # knee\n",
    "qd0 = np.zeros(plant.num_velocities())\n",
    "plant.SetPositions(plant_context,q0)\n",
    "plant.SetVelocities(plant_context,qd0)\n",
    "\n",
    "\n",
    "# Run the simulation a bit\n",
    "simulator.AdvanceTo(0.01)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [
    {
     "output_type": "error",
     "ename": "SystemExit",
     "evalue": "Failure at multibody/plant/tamsi_solver.cc:217 in SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed.",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mSystemExit\u001b[0m                                Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-42-9e3fffc80022>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     23\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     24\u001b[0m \u001b[0;31m# Run the simulation a bit\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 25\u001b[0;31m \u001b[0msimulator\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mAdvanceTo\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m0.1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[0;31mSystemExit\u001b[0m: Failure at multibody/plant/tamsi_solver.cc:217 in SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed."
     ]
    }
   ],
   "source": [
    "%tb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {},
   "outputs": [
    {
     "output_type": "error",
     "ename": "SystemExit",
     "evalue": "Failure at multibody/plant/tamsi_solver.cc:217 in SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed.",
     "traceback": [
      "An exception has occurred, use %tb to see the full traceback.\n",
      "\u001b[0;31mSystemExit\u001b[0m\u001b[0;31m:\u001b[0m Failure at multibody/plant/tamsi_solver.cc:217 in SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed.\n"
     ]
    }
   ],
   "source": [
    "# Run the simulation!\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "False"
      ]
     },
     "metadata": {},
     "execution_count": 33
    }
   ],
   "source": [
    "use_lcm"
   ]
  },
  {
   "source": [
    "## 现象\n",
    "\n",
    "平台与控制器连接正常。\n",
    "\n",
    "控制器不能成功。尝试调节简单控制器的Kp和Kd参数也没有成功。"
   ],
   "cell_type": "markdown",
   "metadata": {}
  }
 ]
}